import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

calib_param = {
    "fExposureTime": 20000.0,  # 相机曝光时间
    'fGain': 15.0  # 相机增益
}

def generate_launch_description():
    camera_dir = get_package_share_directory('hnurm_camera')
    calib_dir = get_package_share_directory('camera_calibration')
    armor_dir = get_package_share_directory('hnurm_armor')

    params_file_camera = LaunchConfiguration('camera_params_file')
    params_file_armor = LaunchConfiguration('armor_params_file')

    cam_node = Node(
        package='hnurm_camera',
        executable='hnurm_camera_node',
        parameters=[params_file_camera, calib_param]
    )

    camera_calibration_node = Node(
        package='camera_calibration',
        executable='cameracalibrator',
        parameters=[params_file_armor],
        arguments=['--size', '11x8', '--square', '0.02', '--no-service-check']
    )

    return LaunchDescription([
        DeclareLaunchArgument(
            'camera_params_file',
            default_value=os.path.join(camera_dir, 'params', 'default.yaml'),
            description='Full path to the ROS2 parameters file to use for the camera'
        ),
        DeclareLaunchArgument(
            'calib_params_file',
            default_value=os.path.join(calib_dir, 'params', 'default.yaml'),
            description='Full path to the ROS2 parameters file to use for the camera calibration'
        ),
        DeclareLaunchArgument(
            'armor_params_file',
            default_value=os.path.join(armor_dir, 'params', 'default.yaml'),
            description='Full path to the ROS2 parameters file to use for the armor'
        ),
        cam_node,
        camera_calibration_node
    ])
